package team240;

import java.util.Stack;

import battlecode.common.*;

public class GroundRobot extends Robots{
	RobotController myRC;
	boolean tracing = false;
	int pathPatience;
	final double modifiedAStarAlpha = 1;
	MapLocation oldGoal;
	public GroundRobot(RobotController inRC) {
		super(inRC);
		myRC = inRC;
		oldGoal = myRC.getLocation();
	}
	public void runGroundRobot() {
		while (true) {
			try {
				RobotType mytype = myRC.getRobotType();
				switch(mytype) {
				case WORKER: Worker myWorker = new Worker(myRC);
					myWorker.runWorker(); break;
		   	 	case SOLDIER: Soldier mySoldier = new Soldier(myRC);
		   	 		mySoldier.runSoldier(); break;
		   	 	case CANNON: Cannon myCannon = new Cannon(myRC);
		   	 		myCannon.runCannon(); break;
		   	 	case CHANNELER: Channeler myChanneler = new Channeler(myRC);
		   	 		myChanneler.runChanneler(); break;
		   	 	default: //error unrecognized type
				}
			}catch(Exception e) {
	            System.out.println("caught exception:");
	            e.printStackTrace();
	        }
		}	
	}
	public void moveInDirection(Direction heading) {
		//move one step in a given direction
		//you need to check if movement is active before calling this method
		MapLocation currentLocation = myRC.getLocation();
		int currentX = currentLocation.getX();
		int currentY = currentLocation.getY();
		MapLocation goal;
		int sensorRadius = myRC.getRobotType().sensorRadius();
		switch (heading) {
		case NORTH:
			goal = new MapLocation(currentX,currentY+sensorRadius);
			moveTowardLocation(goal); break;
		case EAST:
			goal = new MapLocation(currentX+sensorRadius,currentY);
			moveTowardLocation(goal); break;
		case SOUTH:
			goal = new MapLocation(currentX,currentY-sensorRadius);
			moveTowardLocation(goal); break;
		case WEST:
			goal = new MapLocation(currentX-sensorRadius,currentY);
			moveTowardLocation(goal); break;
		case NORTH_EAST:
			goal = new MapLocation(currentX+sensorRadius,currentY+sensorRadius);
			moveTowardLocation(goal); break;
		case NORTH_WEST:
			goal = new MapLocation(currentX-sensorRadius,currentY+sensorRadius);
			moveTowardLocation(goal); break;
		case SOUTH_EAST:
			goal = new MapLocation(currentX+sensorRadius,currentY-sensorRadius);
			moveTowardLocation(goal); break;
		case SOUTH_WEST:
			goal = new MapLocation(currentX-sensorRadius,currentY-sensorRadius);
			moveTowardLocation(goal); break;
		default: //error unexpected direction
		}
	}
	public void moveTowardLocation(MapLocation goal) {
		//move one step closer toward a given location
		//you need to check if movement is active before calling this method
		try {
			if (goal == oldGoal) {	
				if (tracing == false) {
					if (!myPath.empty()) {
						followPath();
					}
					else {
						if (myRC.getLocation() != goal) {
							if (myRC.canSenseSquare(goal)) {
								if (getDistanceSquared(myRC.getLocation(),goal) <= 2) {
									moveOneInDirection(getDirection(myRC.getLocation(),goal));
								}
								else {
									if ((myRC.getRobotType()==RobotType.SOLDIER || myRC.getRobotType()==RobotType.CANNON) && myRC.getDirection()!=getDirection(myRC.getLocation(),goal)) {
										myRC.setDirection(getDirection(myRC.getLocation(),goal));
									}
									else {
										getShortestSensiblePath(goal);
										if (myPath.empty()) {
											//tracing code here
											moveOneInDirection(getDirection(myRC.getLocation(),goal).rotateRight());
										}
										else {
											moveTowardLocation(goal);
										}
									}
								}
							}
							else {
								Direction myHeading = getDirection(myRC.getLocation(),goal);
								if (myHeading != myRC.getDirection()) {
									myRC.setDirection(myHeading);
								}
								else {
									moveTwo(myHeading);
									if(myPath.empty()) {
										//tracing code here
										moveOneInDirection(getDirection(myRC.getLocation(),goal).rotateRight());
									}
									else {
										moveTowardLocation(goal);
									}
								}
							}
						}
					}
				}
				else {
					//insert long, hard, and useless tracing code here
				}
			}
			else {
				oldGoal = goal;
				myPath.clear();
				tracing = false;
			}
		} catch(Exception e) {
			System.out.println("caught exception:");
			e.printStackTrace();
		}
	}
	public void followPath() {
		try {
			MapLocation currentGoal = myPath.pop();
			Direction heading = getDirection(myRC.getLocation(),currentGoal);
			if (myRC.canMove(heading)) {
				if (myRC.getDirection() == heading) {
					myRC.moveForward();
					resetPathPatience();
					
				}
				else if (myRC.getDirection()==heading.opposite()) {
					myRC.moveBackward();
					resetPathPatience();
				}
				else {
					myPath.push(currentGoal);
					myRC.setDirection(heading);
				}
			}
			else {
				if (pathPatience == 0) {
					Stack<MapLocation> filledLocations = new Stack<MapLocation>();
					filledLocations.push(currentGoal);
					MapLocation nextLocation = myPath.pop();
					if (myRC.canSenseSquare(nextLocation)) {
						Robot obstacle = myRC.senseGroundRobotAtLocation(nextLocation);
						while (obstacle != null) {
							filledLocations.push(nextLocation);
							nextLocation = myPath.pop();
							if (myRC.canSenseSquare(nextLocation)) {
								obstacle = myRC.senseGroundRobotAtLocation(nextLocation);
							}
							else {
								Direction newHeading = getDirection(myRC.getLocation(),nextLocation);
								myPath.push(nextLocation);
								while (!filledLocations.empty()) {
									myPath.push(filledLocations.pop());
								}
								if (myRC.getDirection() == newHeading) {
									moveOneInDirection(newHeading);
								}
								else {
									myRC.setDirection(newHeading);
								}
								return;
							}
						}
						Stack<MapLocation> newPath = modifyPath(nextLocation,filledLocations);
						myPath.push(nextLocation);
						if (newPath.empty()) {
							while (!filledLocations.empty()) {
								myPath.push(filledLocations.pop());
							}
							moveOneInDirection(heading);
						}
						else {
							while (!newPath.empty()) {
								myPath.push(newPath.pop());
							}
						}
					}
					else {
						Direction newHeading = getDirection(myRC.getLocation(),nextLocation);
						myPath.push(nextLocation);
						myPath.push(currentGoal);
						if (myRC.getDirection() == newHeading) {
							moveOneInDirection(newHeading);
						}
						else {
							myRC.setDirection(newHeading);
						}
					}
				}
				else {
					myPath.push(currentGoal);
					pathPatience--;
				}
			}
		} catch(Exception e) {
			System.out.println("caught exception:");
			e.printStackTrace();
		}
	}
	public Stack<MapLocation> modifyPath(MapLocation goal,Stack<MapLocation> filledLocations) {
		try {
			//modified version of A* which allows you to send in closed nodes
			//returns a new path to get back onto your old path if the way is blocked
			int myOrthogonalDelay = myRC.getRobotType().moveDelayOrthogonal();
			int myDiagonalDelay = myRC.getRobotType().moveDelayDiagonal();
			MapLocation currentLocation = myRC.getLocation();
			Direction lookDirection = null;
			int waterCounter = 0;
			switch (myRC.getRobotType()) {
			case WORKER: lookDirection = Direction.NORTH; waterCounter = 8; break;
			case SOLDIER: lookDirection = myRC.getDirection().rotateLeft().rotateLeft(); waterCounter = 5; break;
			case CANNON: lookDirection = myRC.getDirection().rotateLeft(); waterCounter = 3; break;
			default: //error unexpected robot type
			}
			MapLocation newLocation = currentLocation.add(lookDirection);
			Robot robotAtNewLocation = myRC.senseGroundRobotAtLocation(newLocation);
			MapNode closed = new MapNode(currentLocation,0,0,null);
			while (!filledLocations.empty()) {
				MapNode makeClosed = new MapNode(filledLocations.pop(),0,0,null);
				closed.pre = makeClosed;
				makeClosed.next = closed;
				closed = makeClosed;
			}
			while ((myRC.senseTerrainTile(newLocation).getType()!=TerrainTile.TerrainType.LAND) || robotAtNewLocation!=null) {
				MapNode makeClosed = new MapNode(newLocation,0,0,null);
				closed.pre = makeClosed;
				makeClosed.next = closed;
				closed = makeClosed;
				lookDirection = lookDirection.rotateRight();
				newLocation = currentLocation.add(lookDirection);
				robotAtNewLocation = myRC.senseGroundRobotAtLocation(newLocation);
				waterCounter--;
				if (waterCounter == 0) {
					System.out.println("Infinite loop in modifyPath avoided. Please tell Beecher if you are reading this.");
					Stack<MapLocation> emptyStack = new Stack<MapLocation>();
					return emptyStack;
				}
			}
			MapNode open;
			if (lookDirection.isDiagonal()) {
				open = new MapNode(newLocation,getMovementPenalty(currentLocation,newLocation)+myOrthogonalDelay,modifiedGuessDistanceToGoal(currentLocation,newLocation,myDiagonalDelay),closed);
			}
			else {
				open = new MapNode(newLocation,getMovementPenalty(currentLocation,newLocation)+myOrthogonalDelay,modifiedGuessDistanceToGoal(currentLocation,newLocation,myOrthogonalDelay),closed);
			}
			for (int k=1;k<waterCounter;k++) {
				lookDirection = lookDirection.rotateRight();
				newLocation = currentLocation.add(lookDirection);
				robotAtNewLocation = myRC.senseGroundRobotAtLocation(newLocation);
				if ((myRC.senseTerrainTile(newLocation).getType()==TerrainTile.TerrainType.LAND) && robotAtNewLocation==null) {
					if (lookDirection.isDiagonal()) {
						MapNode newNode = new MapNode(newLocation,getMovementPenalty(currentLocation,newLocation)+myOrthogonalDelay,modifiedGuessDistanceToGoal(currentLocation,newLocation,myDiagonalDelay),closed);
						open = addNode(open,newNode,closed);
					}
					else {
						MapNode newNode = new MapNode(newLocation,getMovementPenalty(currentLocation,newLocation)+myOrthogonalDelay,modifiedGuessDistanceToGoal(currentLocation,newLocation,myOrthogonalDelay),closed);
						open = addNode(open,newNode,closed);
					}
				}
				else {
					MapNode makeClosed = new MapNode(newLocation,0,0,null);
					closed.pre = makeClosed;
					makeClosed.next = closed;
					closed = makeClosed;
				}
			}
			MapLocation openLocation = open.myLocation;
			double distanceToOpen = open.myDistance;
			while (openLocation != goal) {
				lookDirection = Direction.NORTH;
				for (int m=1;m<3;m++) {
					newLocation = openLocation.add(lookDirection);
					if (myRC.canSenseSquare(newLocation)) {
						if (myRC.senseTerrainTile(newLocation).getType() == TerrainTile.TerrainType.LAND) {
							MapNode newNode = new MapNode(newLocation,distanceToOpen+getMovementPenalty(openLocation,newLocation)+myOrthogonalDelay,modifiedGuessDistanceToGoal(openLocation,newLocation,myOrthogonalDelay),open);
							open = addNode(open,newNode,closed);
						}
					}
					newLocation = openLocation.subtract(lookDirection);
					if (myRC.canSenseSquare(newLocation)) {
						if (myRC.senseTerrainTile(newLocation).getType() == TerrainTile.TerrainType.LAND) {
							MapNode newNode = new MapNode(newLocation,distanceToOpen+getMovementPenalty(openLocation,newLocation)+myOrthogonalDelay,modifiedGuessDistanceToGoal(openLocation,newLocation,myOrthogonalDelay),open);
							open = addNode(open,newNode,closed);
						}
					}
					lookDirection = lookDirection.rotateRight();
					newLocation = openLocation.add(lookDirection);
					if (myRC.canSenseSquare(newLocation)) {
						if (myRC.senseTerrainTile(newLocation).getType() == TerrainTile.TerrainType.LAND) {
							MapNode newNode = new MapNode(newLocation,distanceToOpen+getMovementPenalty(openLocation,newLocation)+myDiagonalDelay,modifiedGuessDistanceToGoal(openLocation,newLocation,myOrthogonalDelay),open);
							open = addNode(open,newNode,closed);
						}
					}
					newLocation = openLocation.subtract(lookDirection);
					if (myRC.canSenseSquare(newLocation)) {
						if (myRC.senseTerrainTile(newLocation).getType() == TerrainTile.TerrainType.LAND) {
							MapNode newNode = new MapNode(newLocation,distanceToOpen+getMovementPenalty(openLocation,newLocation)+myDiagonalDelay,modifiedGuessDistanceToGoal(openLocation,newLocation,myOrthogonalDelay),open);
							open = addNode(open,newNode,closed);
						}
					}
					lookDirection = lookDirection.rotateRight();
				}
				closed.pre = open;
				if (open.next == null) {
					Stack<MapLocation> emptyStack = new Stack<MapLocation>();
					return emptyStack;
				}
				open = open.next;
				openLocation = open.myLocation;
				distanceToOpen = open.myDistance;
			}
			Stack<MapLocation> newPath = new Stack<MapLocation>();
			MapNode traceNode = open.previousNode;
			MapLocation traceLocation = traceNode.myLocation;
			while (traceLocation != currentLocation) {
				newPath.push(traceLocation);
				traceNode = traceNode.previousNode;
				traceLocation = traceNode.myLocation;
			}
			return newPath;
		} catch(Exception e) {
            System.out.println("caught exception:");
            e.printStackTrace();
        }
		Stack<MapLocation> emptyStack = new Stack<MapLocation>();
		return emptyStack;
	}
	public double modifiedGuessDistanceToGoal (MapLocation start, MapLocation goal, int moveDelay) {
		//returns an overestimated guess of the distance between two locations to make A* movement faster
		return (Math.abs(goal.getX()-start.getX())+Math.abs(goal.getY()-start.getY())*moveDelay*(aStarAlpha*modifiedAStarAlpha));
	}
	public void resetPathPatience() {
		//determines how long each robot type will wait for other robots to get out of the way in each status state when following a path
		switch (myRC.getRobotType()) {
		case CANNON:
			switch (myStatus) {
			case COMBAT: 
				pathPatience = 15; 
				break;
			default: //error unrecognized status
			}
		case SOLDIER:
			switch (myStatus) {
			case COMBAT: 
				pathPatience = 15;
				break;
			default: //error unrecognized status
			}
		case WORKER:
			switch (myStatus) {
			case COMBAT: 
				pathPatience = 15;
				break;
			default: //error unrecognized status
			}
		default: //error unrecognized robot type
		}
	}
	public void moveTwo(Direction heading) {
		//finds the shortest 2 square path in the desired direction
		MapLocation myLocation = myRC.getLocation();
		MapLocation first = myLocation.add(heading);
		if (myRC.senseTerrainTile(first).getType() == TerrainTile.TerrainType.LAND) {
			MapLocation second = first.add(heading);
			if (myRC.senseTerrainTile(second).getType() == TerrainTile.TerrainType.LAND) {
				double path1 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,second);
				if (path1 == 0) {
					myPath.push(second);
					myPath.push(first);
				}
				else {
					double lowest = path1;
					int shortest = 1;
					MapLocation third = myLocation.add(heading.rotateLeft());
					if (myRC.senseTerrainTile(third).getType() == TerrainTile.TerrainType.LAND) {
						double path2 = getMovementPenalty(myLocation,third)+getMovementPenalty(third,second);
						if (path2 == 0) {
							myPath.push(second);
							myPath.push(third);
						}
						else {
							if (path2 < lowest) {
								lowest = path2;
								shortest = 2;
							}
							MapLocation fourth = myLocation.add(heading.rotateRight());
							if (myRC.senseTerrainTile(fourth).getType() == TerrainTile.TerrainType.LAND) {
								double path3 = getMovementPenalty(myLocation,fourth)+getMovementPenalty(fourth,second);
								if (path3 == 0) {
									myPath.push(second);
									myPath.push(fourth);
								}
								else {
									if (path3 < lowest) {
										lowest = path3;
										shortest = 3;
									}
									MapLocation fifth = third.add(heading);
									if (myRC.senseTerrainTile(fifth).getType() == TerrainTile.TerrainType.LAND) {
										double path4 = getMovementPenalty(myLocation,third)+getMovementPenalty(third,fifth);
										if (path4 == 0) {
											myPath.push(fifth);
											myPath.push(third);
										}
										else {
											if (path4 < lowest) {
												lowest = path4;
												shortest = 4;
											}
											double path5 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,fifth);
											if (path5 == 0) {
												myPath.push(fifth);
												myPath.push(first);
											}
											else {
												if (path5 < lowest) {
													lowest = path5;
													shortest = 5;
												}
												MapLocation sixth = fourth.add(heading);
												if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
													double path6 = getMovementPenalty(myLocation,fourth)+getMovementPenalty(fourth,sixth);
													if (path6 == 0) {
														myPath.push(sixth);
														myPath.push(fourth);
													}
													else {
														if (path6 < lowest) {
															lowest = path6;
															shortest = 6;
														}
														double path7 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,sixth);
														if (path7 < lowest) {
															myPath.push(sixth);
															myPath.push(first);
														}
														else {
															switch(shortest) {
															case 1: myPath.push(second); myPath.push(first); break;
															case 2: myPath.push(second); myPath.push(third); break;
															case 3: myPath.push(second); myPath.push(fourth); break;
															case 4: myPath.push(fifth); myPath.push(third); break;
															case 5: myPath.push(fifth); myPath.push(first); break;
															case 6: myPath.push(sixth); myPath.push(fourth); break;
															default: //error code
															}
														}
													}
												}
												else {
													//no sixth
													switch(shortest) {
													case 1: myPath.push(second); myPath.push(first); break;
													case 2: myPath.push(second); myPath.push(third); break;
													case 3: myPath.push(second); myPath.push(fourth); break;
													case 4: myPath.push(fifth); myPath.push(third); break;
													case 5: myPath.push(fifth); myPath.push(first); break;
													default: //error code
													}
												}
											}
										}
									}
									else {
										//no fifth
										MapLocation sixth = fourth.add(heading);
										if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
											double path6 = getMovementPenalty(myLocation,fourth)+getMovementPenalty(fourth,sixth);
											if (path6 == 0) {
												myPath.push(sixth);
												myPath.push(fourth);
											}
											else {
												if (path6 < lowest) {
													lowest = path6;
													shortest = 6;
												}
												double path7 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,sixth);
												if (path7 < lowest) {
													myPath.push(sixth);
													myPath.push(first);
												}
												else {
													switch(shortest) {
													case 1: myPath.push(second); myPath.push(first); break;
													case 2: myPath.push(second); myPath.push(third); break;
													case 3: myPath.push(second); myPath.push(fourth); break;
													case 6: myPath.push(sixth); myPath.push(fourth); break;
													default: //error code
													}
												}
											}
										}
										else {
											switch(shortest) {
											case 1: myPath.push(second); myPath.push(first); break;
											case 2: myPath.push(second); myPath.push(third); break;
											case 3: myPath.push(second); myPath.push(fourth); break;
											default: //error code
											}
										}
									}
								}
							}
							else {
								//no fourth
								MapLocation fifth = third.add(heading);
								if (myRC.senseTerrainTile(fifth).getType() == TerrainTile.TerrainType.LAND) {
									double path4 = getMovementPenalty(myLocation,third)+getMovementPenalty(third,fifth);
									if (path4 == 0) {
										myPath.push(fifth);
										myPath.push(third);
									}
									else {
										if (path4 < lowest) {
											lowest = path4;
											shortest = 4;
										}
										double path5 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,fifth);
										if (path5 == 0) {
											myPath.push(fifth);
											myPath.push(first);
										}
										else {
											if (path5 < lowest) {
												lowest = path5;
												shortest = 5;
											}
											MapLocation sixth = fourth.add(heading);
											if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
												double path7 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,sixth);
												if (path7 < lowest) {
													myPath.push(sixth);
													myPath.push(first);
												}
												else {
													switch(shortest) {
													case 1: myPath.push(second); myPath.push(first); break;
													case 2: myPath.push(second); myPath.push(third); break;
													case 4: myPath.push(fifth); myPath.push(third); break;
													case 5: myPath.push(fifth); myPath.push(first); break;
													default: //error code
													}
												}
											}
											else {
												switch(shortest) {
												case 1: myPath.push(second); myPath.push(first); break;
												case 2: myPath.push(second); myPath.push(third); break;
												case 4: myPath.push(fifth); myPath.push(third); break;
												case 5: myPath.push(fifth); myPath.push(first); break;
												default: //error code
												}
											}
										}
									}
								}
								else {
									MapLocation sixth = fourth.add(heading);
									if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
										double path7 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,sixth);
										if (path7 < lowest) {
											myPath.push(sixth);
											myPath.push(first);
										}
										else {
											switch(shortest) {
											case 1: myPath.push(second); myPath.push(first); break;
											case 2: myPath.push(second); myPath.push(third); break;
											default: //error code
											}
										}
									}
									else {
										switch(shortest) {
										case 1: myPath.push(second); myPath.push(first); break;
										case 2: myPath.push(second); myPath.push(third); break;
										default: //error code
										}
									}
								}
							}
						}
					}
					else {
						//no third
						MapLocation fourth = myLocation.add(heading.rotateRight());
						if (myRC.senseTerrainTile(fourth).getType() == TerrainTile.TerrainType.LAND) {
							double path3 = getMovementPenalty(myLocation,fourth)+getMovementPenalty(fourth,second);
							if (path3 == 0) {
								myPath.push(second);
								myPath.push(fourth);
							}
							else {
								if (path3 < lowest) {
									lowest = path3;
									shortest = 3;
								}
								MapLocation fifth = third.add(heading);
								if (myRC.senseTerrainTile(fifth).getType() == TerrainTile.TerrainType.LAND) {
									double path5 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,fifth);
									if (path5 == 0) {
										myPath.push(fifth);
										myPath.push(first);
									}
									else {
										if (path5 < lowest) {
											lowest = path5;
											shortest = 5;
										}
										MapLocation sixth = fourth.add(heading);
										if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
											double path6 = getMovementPenalty(myLocation,fourth)+getMovementPenalty(fourth,sixth);
											if (path6 == 0) {
												myPath.push(sixth);
												myPath.push(fourth);
											}
											else {
												if (path6 < lowest) {
													lowest = path6;
													shortest = 6;
												}
												double path7 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,sixth);
												if (path7 < lowest) {
													myPath.push(sixth);
													myPath.push(first);
												}
												else {
													switch(shortest) {
													case 1: myPath.push(second); myPath.push(first); break;
													case 3: myPath.push(second); myPath.push(fourth); break;
													case 5: myPath.push(fifth); myPath.push(first); break;
													case 6: myPath.push(sixth); myPath.push(fourth); break;
													default: //error code
													}
												}
											}
										}
										else {
											switch(shortest) {
											case 1: myPath.push(second); myPath.push(first); break;
											case 3: myPath.push(second); myPath.push(fourth); break;
											case 5: myPath.push(fifth); myPath.push(first); break;
											default: //error code
											}
										}
									}
								}
								else {
									MapLocation sixth = fourth.add(heading);
									if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
										double path6 = getMovementPenalty(myLocation,fourth)+getMovementPenalty(fourth,sixth);
										if (path6 == 0) {
											myPath.push(sixth);
											myPath.push(fourth);
										}
										else {
											if (path6 < lowest) {
												lowest = path6;
												shortest = 6;
											}
											double path7 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,sixth);
											if (path7 < lowest) {
												myPath.push(sixth);
												myPath.push(first);
											}
											else {
												switch(shortest) {
												case 1: myPath.push(second); myPath.push(first); break;
												case 3: myPath.push(second); myPath.push(fourth); break;
												case 6: myPath.push(sixth); myPath.push(fourth); break;
												default: //error code
												}
											}
										}
									}
									else {
										switch(shortest) {
										case 1: myPath.push(second); myPath.push(first); break;
										case 3: myPath.push(second); myPath.push(fourth); break;
										default: //error code
										}
									}
								}
							}
						}
						else {
							MapLocation fifth = third.add(heading);
							if (myRC.senseTerrainTile(fifth).getType() == TerrainTile.TerrainType.LAND) {
								double path5 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,fifth);
								if (path5 == 0) {
									myPath.push(fifth);
									myPath.push(first);
								}
								else {
									if (path5 < lowest) {
										lowest = path5;
										shortest = 5;
									}
									MapLocation sixth = fourth.add(heading);
									if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
										double path7 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,sixth);
										if (path7 < lowest) {
											myPath.push(sixth);
											myPath.push(first);
										}
										else {
											switch(shortest) {
											case 1: myPath.push(second); myPath.push(first); break;
											case 5: myPath.push(fifth); myPath.push(first); break;
											default: //error code
											}
										}
									}
									else {
										switch(shortest) {
										case 1: myPath.push(second); myPath.push(first); break;
										case 5: myPath.push(fifth); myPath.push(first); break;
										default: //error code
										}
									}
								}
							}
							else {
								MapLocation sixth = fourth.add(heading);
								if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
									double path7 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,sixth);
									if (path7 < lowest) {
										myPath.push(sixth);
										myPath.push(first);
									}
									else {
										switch(shortest) {
										case 1: myPath.push(second); myPath.push(first); break;
										default: //error code
										}
									}
								}
								else {
									switch(shortest) {
									case 1: myPath.push(second); myPath.push(first); break;
									default: //error code
									}
								}
							}
						}
					}
				}
			}
			else {
				//no second
				MapLocation third = myLocation.add(heading.rotateLeft());
				if (myRC.senseTerrainTile(third).getType() == TerrainTile.TerrainType.LAND) {
					MapLocation fourth = myLocation.add(heading.rotateRight());
					if (myRC.senseTerrainTile(fourth).getType() == TerrainTile.TerrainType.LAND) {
						MapLocation fifth = third.add(heading);
						if (myRC.senseTerrainTile(fifth).getType() == TerrainTile.TerrainType.LAND) {
							double path4 = getMovementPenalty(myLocation,third)+getMovementPenalty(third,fifth);
							if (path4 == 0) {
								myPath.push(fifth);
								myPath.push(third);
							}
							else {
								double lowest = path4;
								int shortest = 4;
								if (path4 < lowest) {
									lowest = path4;
									shortest = 4;
								}
								double path5 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,fifth);
								if (path5 == 0) {
									myPath.push(fifth);
									myPath.push(first);
								}
								else {
									if (path5 < lowest) {
										lowest = path5;
										shortest = 5;
									}
									MapLocation sixth = fourth.add(heading);
									if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
										double path6 = getMovementPenalty(myLocation,fourth)+getMovementPenalty(fourth,sixth);
										if (path6 == 0) {
											myPath.push(sixth);
											myPath.push(fourth);
										}
										else {
											if (path6 < lowest) {
												lowest = path6;
												shortest = 6;
											}
											double path7 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,sixth);
											if (path7 < lowest) {
												myPath.push(sixth);
												myPath.push(first);
											}
											else {
												switch(shortest) {
												case 4: myPath.push(fifth); myPath.push(third); break;
												case 5: myPath.push(fifth); myPath.push(first); break;
												case 6: myPath.push(sixth); myPath.push(fourth); break;
												default: //error code
												}
											}
										}
									}
									else {
										switch(shortest) {
										case 4: myPath.push(fifth); myPath.push(third); break;
										case 5: myPath.push(fifth); myPath.push(first); break;
										default: //error code
										}
									}
								}
							}
						}
						else {
							MapLocation sixth = fourth.add(heading);
							if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
								double path6 = getMovementPenalty(myLocation,fourth)+getMovementPenalty(fourth,sixth);
								if (path6 == 0) {
									myPath.push(sixth);
									myPath.push(fourth);
								}
								else {
									double lowest = path6;
									int shortest = 6;
									if (path6 < lowest) {
										lowest = path6;
										shortest = 6;
									}
									double path7 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,sixth);
									if (path7 < lowest) {
										myPath.push(sixth);
										myPath.push(first);
									}
									else {
										switch(shortest) {
										case 6: myPath.push(sixth); myPath.push(fourth); break;
										default: //error code
										}
									}
								}
							}
						}
					}
					else {
						MapLocation fifth = third.add(heading);
						if (myRC.senseTerrainTile(fifth).getType() == TerrainTile.TerrainType.LAND) {
							double path4 = getMovementPenalty(myLocation,third)+getMovementPenalty(third,fifth);
							if (path4 == 0) {
								myPath.push(fifth);
								myPath.push(third);
							}
							else {
								double lowest = path4;
								int shortest = 4;
								if (path4 < lowest) {
									lowest = path4;
									shortest = 4;
								}
								double path5 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,fifth);
								if (path5 == 0) {
									myPath.push(fifth);
									myPath.push(first);
								}
								else {
									if (path5 < lowest) {
										lowest = path5;
										shortest = 5;
									}
									MapLocation sixth = fourth.add(heading);
									if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
										double path7 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,sixth);
										if (path7 < lowest) {
											myPath.push(sixth);
											myPath.push(first);
										}
										else {
											switch(shortest) {
											case 4: myPath.push(fifth); myPath.push(third); break;
											case 5: myPath.push(fifth); myPath.push(first); break;
											default: //error code
											}
										}
									}
									else {
										switch(shortest) {
										case 4: myPath.push(fifth); myPath.push(third); break;
										case 5: myPath.push(fifth); myPath.push(first); break;
										default: //error code
										}
									}
								}
							}
						}
						else {
							MapLocation sixth = fourth.add(heading);
							if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
								myPath.push(sixth);
								myPath.push(first);
							}
						}
					}
				}
				else {
					//no third
					MapLocation fourth = myLocation.add(heading.rotateRight());
					if (myRC.senseTerrainTile(fourth).getType() == TerrainTile.TerrainType.LAND) {
						MapLocation fifth = third.add(heading);
						if (myRC.senseTerrainTile(fifth).getType() == TerrainTile.TerrainType.LAND) {
							double path5 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,fifth);
							if (path5 == 0) {
								myPath.push(fifth);
								myPath.push(first);
							}
							else {
								double lowest = path5;
								int shortest = 5;
								if (path5 < lowest) {
									lowest = path5;
									shortest = 5;
								}
								MapLocation sixth = fourth.add(heading);
								if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
									double path6 = getMovementPenalty(myLocation,fourth)+getMovementPenalty(fourth,sixth);
									if (path6 == 0) {
										myPath.push(sixth);
										myPath.push(fourth);
									}
									else {
										if (path6 < lowest) {
											lowest = path6;
											shortest = 6;
										}
										double path7 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,sixth);
										if (path7 < lowest) {
											myPath.push(sixth);
											myPath.push(first);
										}
										else {
											switch(shortest) {
											case 5: myPath.push(fifth); myPath.push(first); break;
											case 6: myPath.push(sixth); myPath.push(fourth); break;
											default: //error code
											}
										}
									}
								}
								else {
									switch(shortest) {
									case 5: myPath.push(fifth); myPath.push(first); break;
									default: //error code
									}
								}
							}
						}
						else {
							MapLocation sixth = fourth.add(heading);
							if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
								double path6 = getMovementPenalty(myLocation,fourth)+getMovementPenalty(fourth,sixth);
								if (path6 == 0) {
									myPath.push(sixth);
									myPath.push(fourth);
								}
								else {
									double lowest = path6;
									int shortest = 6;
									if (path6 < lowest) {
										lowest = path6;
										shortest = 6;
									}
									double path7 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,sixth);
									if (path7 < lowest) {
										myPath.push(sixth);
										myPath.push(first);
									}
									else {
										switch(shortest) {
										case 6: myPath.push(sixth); myPath.push(fourth); break;
										default: //error code
										}
									}
								}
							}
						}
					}
					else {
						MapLocation fifth = third.add(heading);
						if (myRC.senseTerrainTile(fifth).getType() == TerrainTile.TerrainType.LAND) {
							double path5 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,fifth);
							if (path5 == 0) {
								myPath.push(fifth);
								myPath.push(first);
							}
							else {
								double lowest = path5;
								int shortest = 5;
								if (path5 < lowest) {
									lowest = path5;
									shortest = 5;
								}
								MapLocation sixth = fourth.add(heading);
								if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
									double path7 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,sixth);
									if (path7 < lowest) {
										myPath.push(sixth);
										myPath.push(first);
									}
									else {
										switch(shortest) {
										case 5: myPath.push(fifth); myPath.push(first); break;
										default: //error code
										}
									}
								}
								else {
									switch(shortest) {
									case 5: myPath.push(fifth); myPath.push(first); break;
									default: //error code
									}
								}
							}
						}
						else {
							MapLocation sixth = fourth.add(heading);
							if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
								myPath.push(sixth);
								myPath.push(first);
							}
						}
					}
				}
			}
		}
		else {
			//no first
			MapLocation second = first.add(heading);
			if (myRC.senseTerrainTile(second).getType() == TerrainTile.TerrainType.LAND) {
				MapLocation third = myLocation.add(heading.rotateLeft());
				if (myRC.senseTerrainTile(third).getType() == TerrainTile.TerrainType.LAND) {
					double path2 = getMovementPenalty(myLocation,third)+getMovementPenalty(third,second);
					if (path2 == 0) {
						myPath.push(second);
						myPath.push(third);
					}
					else {
						double lowest = path2;
						int shortest = 2;
						if (path2 < lowest) {
							lowest = path2;
							shortest = 2;
						}
						MapLocation fourth = myLocation.add(heading.rotateRight());
						if (myRC.senseTerrainTile(fourth).getType() == TerrainTile.TerrainType.LAND) {
							double path3 = getMovementPenalty(myLocation,fourth)+getMovementPenalty(fourth,second);
							if (path3 == 0) {
								myPath.push(second);
								myPath.push(fourth);
							}
							else {
								if (path3 < lowest) {
									lowest = path3;
									shortest = 3;
								}
								MapLocation fifth = third.add(heading);
								if (myRC.senseTerrainTile(fifth).getType() == TerrainTile.TerrainType.LAND) {
									double path4 = getMovementPenalty(myLocation,third)+getMovementPenalty(third,fifth);
									if (path4 == 0) {
										myPath.push(fifth);
										myPath.push(third);
									}
									else {
										if (path4 < lowest) {
											lowest = path4;
											shortest = 4;
										}
										MapLocation sixth = fourth.add(heading);
										if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
											double path6 = getMovementPenalty(myLocation,fourth)+getMovementPenalty(fourth,sixth);
											if (path6 < lowest) {
												myPath.push(sixth);
												myPath.push(fourth);
											}
											else {
												switch(shortest) {
												case 2: myPath.push(second); myPath.push(third); break;
												case 3: myPath.push(second); myPath.push(fourth); break;
												case 4: myPath.push(fifth); myPath.push(third); break;
												default: //error code
												}
											}
										}
										else {
											switch(shortest) {
											case 2: myPath.push(second); myPath.push(third); break;
											case 3: myPath.push(second); myPath.push(fourth); break;
											case 4: myPath.push(fifth); myPath.push(third); break;
											default: //error code
											}
										}
									}
								}
								else {
									MapLocation sixth = fourth.add(heading);
									if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
										double path6 = getMovementPenalty(myLocation,fourth)+getMovementPenalty(fourth,sixth);
										if (path6 < lowest) {
											myPath.push(sixth);
											myPath.push(fourth);
										}
										else {
											switch(shortest) {
											case 2: myPath.push(second); myPath.push(third); break;
											case 3: myPath.push(second); myPath.push(fourth); break;
											default: //error code
											}
										}
									}
									else {
										switch(shortest) {
										case 2: myPath.push(second); myPath.push(third); break;
										case 3: myPath.push(second); myPath.push(fourth); break;
										default: //error code
										}
									}
								}
							}
						}
						else {
							MapLocation fifth = third.add(heading);
							if (myRC.senseTerrainTile(fifth).getType() == TerrainTile.TerrainType.LAND) {
								double path4 = getMovementPenalty(myLocation,third)+getMovementPenalty(third,fifth);
								if (path4 < lowest) {
									myPath.push(fifth);
									myPath.push(third);
								}
								else {
																
									switch(shortest) {
									case 2: myPath.push(second); myPath.push(third); break;
									default: //error code
									}
								}
							}
							else {
								MapLocation sixth = fourth.add(heading);
								if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
									double path7 = getMovementPenalty(myLocation,first)+getMovementPenalty(first,sixth);
									if (path7 < lowest) {
										myPath.push(sixth);
										myPath.push(first);
									}
									else {
										switch(shortest) {
										case 2: myPath.push(second); myPath.push(third); break;
										default: //error code
										}
									}
								}
								else {
									switch(shortest) {
									case 2: myPath.push(second); myPath.push(third); break;
									default: //error code
									}
								}
							}
						}
					}
				}
				else {
					//no third
					MapLocation fourth = myLocation.add(heading.rotateRight());
					if (myRC.senseTerrainTile(fourth).getType() == TerrainTile.TerrainType.LAND) {
						double path3 = getMovementPenalty(myLocation,fourth)+getMovementPenalty(fourth,second);
						if (path3 == 0) {
							myPath.push(second);
							myPath.push(fourth);
						}
						else {
							double lowest = path3;
							int shortest = 3;
							if (path3 < lowest) {
								lowest = path3;
								shortest = 3;
							}
							MapLocation fifth = third.add(heading);
							if (myRC.senseTerrainTile(fifth).getType() == TerrainTile.TerrainType.LAND) {
								MapLocation sixth = fourth.add(heading);
								if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
									double path6 = getMovementPenalty(myLocation,fourth)+getMovementPenalty(fourth,sixth);
									if (path6 < lowest) {
										myPath.push(sixth);
										myPath.push(fourth);
									}
									else {
										switch(shortest) {
										case 3: myPath.push(second); myPath.push(fourth); break;
										default: //error code
										}
									}
								}
								else {
									switch(shortest) {
									case 3: myPath.push(second); myPath.push(fourth); break;
									default: //error code
									}
								}
							}
							else {
								MapLocation sixth = fourth.add(heading);
								if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
									double path6 = getMovementPenalty(myLocation,fourth)+getMovementPenalty(fourth,sixth);
									if (path6 < lowest) {
										myPath.push(sixth);
										myPath.push(fourth);
									}
									else {
										switch(shortest) {
										case 3: myPath.push(second); myPath.push(fourth); break;
										case 6: myPath.push(sixth); myPath.push(fourth); break;
										default: //error code
										}
									}
								}
								else {
									switch(shortest) {
									case 3: myPath.push(second); myPath.push(fourth); break;
									default: //error code
									}
								}
							}
						}
					}
				}
			}
			else {
				//no second
				MapLocation third = myLocation.add(heading.rotateLeft());
				if (myRC.senseTerrainTile(third).getType() == TerrainTile.TerrainType.LAND) {
					MapLocation fourth = myLocation.add(heading.rotateRight());
					if (myRC.senseTerrainTile(fourth).getType() == TerrainTile.TerrainType.LAND) {
						MapLocation fifth = third.add(heading);
						if (myRC.senseTerrainTile(fifth).getType() == TerrainTile.TerrainType.LAND) {
							double path4 = getMovementPenalty(myLocation,third)+getMovementPenalty(third,fifth);
							if (path4 == 0) {
								myPath.push(fifth);
								myPath.push(third);
							}
							else {
								double lowest = path4;
								int shortest = 4;
								if (path4 < lowest) {
									lowest = path4;
									shortest = 4;
								}
								MapLocation sixth = fourth.add(heading);
								if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
									double path6 = getMovementPenalty(myLocation,fourth)+getMovementPenalty(fourth,sixth);
									if (path6 < lowest) {
										myPath.push(sixth);
										myPath.push(fourth);
									}
									else {
										switch(shortest) {
										case 4: myPath.push(fifth); myPath.push(third); break;
										default: //error code
										}
									}
								}
								else {
									switch(shortest) {
									case 4: myPath.push(fifth); myPath.push(third); break;
									default: //error code
									}
								}
							}
						}
						else {
							MapLocation sixth = fourth.add(heading);
							if (myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
								myPath.push(sixth);
								myPath.push(fourth);
							}
						}
					}
					else {
						MapLocation fifth = third.add(heading);
						if (myRC.senseTerrainTile(fifth).getType() == TerrainTile.TerrainType.LAND) {
							myPath.push(fifth);
							myPath.push(third);
						}
					}
				}
				else {
					//no third
					MapLocation fourth = myLocation.add(heading.rotateRight());
					MapLocation sixth = fourth.add(heading);
					if (myRC.senseTerrainTile(fourth).getType() == TerrainTile.TerrainType.LAND && myRC.senseTerrainTile(sixth).getType() == TerrainTile.TerrainType.LAND) {
						myPath.push(sixth);
						myPath.push(fourth);
					}
				}
			}
		}
	}
}

